#include "zf_common_headfile.h"

#define INCURVITY_STEP 5 //曲率步长

Str_Inflection_point  Inflection_point;


uint8_t Detect_Boundary_R_Num(uint8_t (*p),uint8_t big,uint8_t small)
{
uint8_t sum=0,di;
for(di=big;di>=small;di--){
    if(p[di] ==COL-1)sum++; 
}
return sum;
}

uint8_t Detect_Boundary_L_Num(uint8_t (*p),uint8_t big,uint8_t small)
{
uint8_t sum=0,di;
for(di=big;di>=small;di--){
    if(p[di] ==0)sum++; 
}
return sum;
}



void find_right_line(unsigned char *img,uint8_t begin_y,uint8_t end_y)
{
unsigned char tempr_c0;//右边线 列比较 当前存储像素值
unsigned char tempr_c1;//右边线 列比较 比较存储像素值
unsigned char ry,rx;
if(end_y<COL_STEP){end_y=COL_STEP;} //限幅保护

   for (ry = begin_y; ry >= end_y; ry--)//对应坐标轴系y
   {
      for (rx =Image_DataStruct.begin_x; rx < COL_End; rx++)//对应坐标轴系x
      {
        tempr_c0 = img[ry*COL+rx];
        tempr_c1 = img[ry*COL+rx+COL_STEP];
         if(rx+COL_STEP>=COL_End) 
         {
            iptsr[ry]=COL_End;
            break;
        }
        if(tempr_c1<=Image_DataStruct.white_min_point) 
        {
                iptsr[ry]=rx;
                break;
        }

         if( Diff_Ratio(tempr_c0 , tempr_c1) >= Image_DataStruct.Diff_Ratio)
         {
            iptsL[ry]=rx;
            break;
         }
      }
   }
}

void find_left_line(unsigned char * img,uint8_t begin_y,uint8_t end_y)
{
unsigned char templ_c0;//左边线 列比较 当前存储像素值
unsigned char templ_c1;//左边线 列比较 比较存储像素值
unsigned char ly,lx;

if(end_y<COL_STEP){end_y=COL_STEP;} //限幅保护

   for (ly = begin_y; ly >= end_y; ly--)//对应坐标轴系y
   {
      for (lx =Image_DataStruct.begin_x; lx > COL_Start; lx--)//对应坐标轴系x
      {
        templ_c0 = img[ly*COL+lx];
        templ_c1 = img[ly*COL+lx-COL_STEP];
         if(lx-COL_STEP<=COL_Start) 
         {
            iptsL[ly]=0;
            break;
        }
        if(templ_c1<=Image_DataStruct.white_min_point) //图像后半部分
        {
                iptsL[ly]=lx;
                break;
        }

         if( Diff_Ratio(templ_c0 , templ_c1) >= Image_DataStruct.Diff_Ratio)
         {
            iptsL[ly]=lx;
            break;
         }
      }
   }
}



/// @brief 单调检测 检测40行起步(对于120行的图像来说)
/// @param big 
/// @param small 
/// @return 1内倾 -1不全内倾
int monotony_L(unsigned char big,unsigned char small)
{
if(small<=ROW_Start-rpts_num){small = ROW_Start-rpts_num+LSC_STEP;}

for (unsigned char i = big; i >=small; i--)
{

if(l_trend[i] == 2)continue;
else{return -1;}
}
return 1;
}

/// @brief 单调检测
/// @param big 
/// @param small 
/// @return 1内倾 -1不全内倾
int monotony_r(unsigned char big,unsigned char small)
{
if(small<=ROW_Start-rpts_num){small = ROW_Start-rpts_num+LSC_STEP;}

for (unsigned char i = big; i >=small; i--)
{
if(r_trend[i] == 1)continue;
else{return -1;}
}
return 1;
}

/// @brief 丢线检测
/// @param big 
/// @param small 
/// @return -1全丢 其它值就是没丢数
int mislaid_r(unsigned char big,unsigned char small)
{
    int sum=0;
for (unsigned char i = big; i >=small; i--)
{
    if(iptsr[i]>=COL_End-1)continue; //不能使用ROW_Lose 那是只丢右
    else{sum++; /* if (System_DataStruct.Element_Flag==RIGHT_CIRCLE_IN){printf("  not lose %d: ",i);} */}
}

if (sum==0)
{
    return -1;//全部丢线
}
else
{
    return sum;//返回没丢数
}

}

int mislaid_l(unsigned char big,unsigned char small)
{
    int sum=0;
for(unsigned char i=big; i>=small; i--)
{
    if(iptsL[i]<=COL_Start){  continue; }
    else {sum++;}
}

if(sum==0)
{
    return -1;
}
else
{
    return sum;
}

}

/// @brief 十字判断,主要应对斜入十字
/// @param  必须放在环岛之后,捡环岛不要的
void cross_process(void)
{
if(System_DataStruct.Element_Flag == ELEMENMT_NONE || System_DataStruct.Element_Flag == CROSS_IN)
{

        unsigned char i;
        unsigned char left_cross_down=0;
        unsigned char left_cross_up=0;
        unsigned char right_cross_down=0;
        unsigned char right_cross_up=0;
        for ( i = ROW_Start-10; i >= ROW_Start-rpts_num+10; i--) //末尾10行不考虑
        {  
            //右斜入十字
            if( iptsL[i+1] >iptsL[i] && iptsL[i+2]>iptsL[i]) //毫不夸张的说,曲率计算可以说在弯道时时计算了,后续有待完善
            {
                int l_monotony = monotony_L(i+10,i+3); //不能每次都进行单调计算,开销太大,而且没有必要
                if(l_monotony == 1) //前段多行内倾
                {
                    // printf("detect_icurvity(i+2,5,20,0): %d \n",detect_icurvity(i+2,5,20,0));

                    if(detect_icurvity(i+2,5,20,0) <=-35)
                    {
                        System_DataStruct.Element_Flag=CROSS_IN;
                        left_cross_down = i+3;
                        printf("find left_cross \n");
                        break; //找到退出
                    }
                    else
                    {
                        System_DataStruct.Element_Flag = ELEMENMT_NONE;
                    }
                }

            }
        }
        for ( i = ROW_Start-10; i >= ROW_Start-rpts_num+10; i--) //末尾10行不考虑
        {
            if( iptsr[i+1] <iptsr[i] && iptsr[i+2]<iptsr[i])
            {
                int r_monotony = monotony_r(i+10,i+3); //不能每次都进行单调计算,开销太大,而且没有必要
                if(r_monotony == 1) //前段多行内倾
                {

                    if(detect_icurvity(i+2,5,20,1) >=35)
                    {
                        System_DataStruct.Element_Flag=CROSS_IN;
                        right_cross_down = i+3;
                        break; //找到退出
                    }
                    else
                    {
                        System_DataStruct.Element_Flag = ELEMENMT_NONE;
                    }
                }
            }
        }
/*******************************************十字处理阶段*****************************************************************/
        if(System_DataStruct.Element_Flag == CROSS_IN)
        {
            if(left_cross_down !=0)
            {
                float slope,intercept;
                bool flag_l=1;
                for(i=left_cross_down-3; i>ROW_Start-rpts_num;i--) //下拐点找的可能往后一点,所以这里在减去一点
                {

                    if(iptsL[i]<=iptsL[left_cross_down])continue; //在斜入时会有看不到上方的情况有待考虑
                    else
                    {
                        left_cross_up=i;
                        slope = (float)(left_cross_up-left_cross_down)/(iptsL[left_cross_up]-iptsL[left_cross_down]);
                        intercept = left_cross_down-slope*iptsL[left_cross_down];
                        Patch_Line_Points(left_cross_down,left_cross_up,slope,intercept,iptsL);
                        // printf("%d iptsL[i]: %d iptsL{left_cross_down]: %d\n",i,iptsL[i],iptsL[left_cross_down]);
                        flag_l=0;
                        break;
                    }
                }

                if(flag_l)
                {
                    printf("test: %d\n",left_cross_down);
                    left_cross_up=ROW_Start-rpts_num;
                    for(i=left_cross_down; i>=ROW_Start-rpts_num;i--)
                    {
                        iptsL[i] = (iptsL[i+1]+1)>=COL_End?COL_End:(iptsL[i+1]+1);

                    }
                }
            }
            if(right_cross_down !=0)
            {
                float slope,intercept;
                bool flag_r=1;
                for(i=right_cross_down-3; i>ROW_Start-rpts_num;i--) //下拐点找的可能往后一点,所以这里在减去一点
                {

                    if(iptsr[i]>=iptsr[right_cross_down])continue; //在斜入时会有看不到上方的情况有待考虑
                    else
                    {
                        right_cross_up=i;
                        slope = (float)(right_cross_up-right_cross_down)/(iptsr[right_cross_up]-iptsr[right_cross_down]);
                        intercept = right_cross_down-slope*iptsr[right_cross_down];
                        Patch_Line_Points(right_cross_down,right_cross_up,slope,intercept,iptsr);
                        // printf("%d iptsL[i]: %d iptsL{right_cross_down]: %d\n",i,iptsL[i],iptsL[right_cross_down]);
                        flag_r=0;
                        break;
                    }
                }

                if(flag_r)
                {
                    right_cross_up=ROW_Start-rpts_num;
                    for(i=right_cross_down; i>=ROW_Start-rpts_num;i--)
                    {
                        iptsr[i] = (iptsr[i+1]-1)<=COL_Start?COL_Start:(iptsr[i+1]-1);

                    }
                }
            }

        }


/***********************************************中线合成阶段**************************************************************************/
if(System_DataStruct.Element_Flag == CROSS_IN)
{
    if(right_cross_down!=0 && left_cross_down!=0)
    {
        for(i = max(left_cross_down,right_cross_down);i>=min(left_cross_up,right_cross_up);i--)
        {
        rpts[i] = (iptsL[i]+iptsr[i])/2;
        }
    }
    else if(right_cross_down!=0)
    {
        for(i = right_cross_down;i>=right_cross_up;i--)
        {
        rpts[i] = (iptsL[i]+iptsr[i])/2;
        }
    }
    else if(left_cross_down!=0)
    {
        for(i = left_cross_down;i>=left_cross_up;i--)
        {
        rpts[i] = (iptsL[i]+iptsr[i])/2;
        }
    }

}
}

}



/// @brief  环岛判定
/// @param  首先判断丢线,其次判断单调,然后判断异路单调,然后曲率判断
void Island_Judge(void)
{
    unsigned char i;
if(Image_DataStruct.lst_rows>=ROW/2)
{
    for ( i = ROW_Start-20; i >= ROW_Start-rpts_num+10; i--) //末尾20行不考虑 顶格10行不考虑
    {  
        int r_mislaid = mislaid_r(i-1,i-4) ;
        int l_mislaid = mislaid_l(i-1,i-4);

        if(r_mislaid==-1 || (iptsr[i-1]-iptsr[i])>=20 ) //预先判断 ,这里找不到不能break,需要遍历
        {
            int r_monotony = monotony_r(i+10,i+1); //不能每次都进行单调计算,开销太大,而且没有必要
            if(r_monotony ==1) //满足前四行丢线,后10行单调,不只有环岛还有十字
            {
                int l_monotony = monotony_L(i+10,i-30);
                if(l_monotony == 1)
                {
                    if(detect_icurvity(i,5,20,1) >=40)
                    {
                        // currentState =IN_PROGRAM ;
                        System_DataStruct.Element_Flag=RIGHT_CIRCLE_IN;
                        System_DataStruct.CIRCLE_Flag=CIRCLE_PREV_D_M;
                        return; //找到退出
                    }

                }//不满足异路40行单调,正入十字就在其中
            break; 
            }
        }
        //左环岛
        else if(l_mislaid==-1 || (iptsL[i]-iptsL[i-1])>=20)
        {
            int l_monotony = monotony_L(i+10,i+1); //不能每次都进行单调计算,开销太大,而且没有必要
            
            if(l_monotony ==1) //满足前四行丢线,后10行单调,不只有环岛还有十字
            {
                int r_monotony = monotony_r(i+10,i-30);
                if(r_monotony == 1) //对于左环岛而言,异路边线应该全部内倾
                {
                    if(detect_icurvity(i,5,20,0) <=-40)
                    {
                        // system("pause");
                        System_DataStruct.Element_Flag=LEFT_CIRCLE_IN;
                        System_DataStruct.CIRCLE_Flag=CIRCLE_PREV_D_M;
                        return; //找到退出
                    }

                }//不满足异路40行单调,正入十字就在其中
            break; 
            }
        }
    }

}


    
}

#define DETECT_LOST_LINE_ARC 19 //19代表20行 检测丢线行数
//右环岛
#define  Width_P_R      (0.58) // 0.48 0.65//越小越往右倾斜 越大越垂直
#define  WIDTH_D_R      (4.36) // //越小 越靠近左侧
//圆弧
#define  WIDTH_P_ARC_R  (0.48)
#define  WIDTH_D_ARC_R  (3.36) //-27.92
//出环
#define  WIDTH_P_OUT_R  (0.48) // iptsL[]+(px+d)
#define  WIDTH_D_OUT_R  (3.36) //-35.99

/******************** 左 环 岛 ******************************************/
#define  Width_P_L      (0.48) //0.48  //越小越往右倾斜 越大越垂直
#define  WIDTH_D_L      (5.36) //3.36  越小 越靠近右侧

#define  WIDTH_P_ARC_L  (0.48)
#define  WIDTH_D_ARC_L  (3.36) //-27.99

#define  WIDTH_P_OUT_L (0.48)  // iptsr[]-(px+d)
#define  WIDTH_D_OUT_L (3.36) //越小  越靠近 右边线

/***********************************************************************/
#define OUT_D_INCREASE 3 //取决于摄像头俯仰角 平视就增大到3 俯视就2
#define PREV_ARC_INCREASE 1 //圆弧后续自增 是否想提前入环?
#define PREV_ARC_MONOTONY 1 // 1:允许交错 0:不允许交错 圆弧路段是否允许偏移线段与理论中线产生交错 0不允许 只允许圆弧路段从理论中线开始单调

void Island_Process(void)
{
   float slope,intercept;
   unsigned char mi;
   int lost_line_r=0;
   int lost_line_l=0;
   int r_monotony;
   int l_monotony;


switch(System_DataStruct.Element_Flag)
{
    case RIGHT_CIRCLE_IN:

                            lost_line_r = mislaid_r(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC); //119 100
                            lost_line_l = mislaid_l(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC);
                            r_monotony  = monotony_r(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC);
                            l_monotony  = monotony_L(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC); // test excess
//                            System_DataStruct.test = lost_line_l;
//                            ips200_show_int(40, ROW+20, lost_line_r, 2);
                            if(lost_line_r !=-1 && System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_D_M) //没有全丢,使用合成的中线
                            {
                                    for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_R+WIDTH_D_R); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = iptsL[mi]+track_Width[mi];
                                        rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                    }

                            }
                        /*******************************************************************************************************************************************/
                            else if( (lost_line_r ==-1) && (System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_D_M ) ) //底部20行全丢 或者得线数少于3 下拐点和圆弧路段
                            {
//                                gpio_toggle_level(BUZZER_PIN);

                                System_DataStruct.CIRCLE_Flag=CIRCLE_PREV_M;
                                for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_R+WIDTH_D_R); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = iptsL[mi]+track_Width[mi];
                                        rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                    }

                            }
                            else if(System_DataStruct.CIRCLE_Flag==CIRCLE_PREV_M) //入圆弧前 //test excess
                            {
                        //            ips200_show_int(20, ROW,22, 3);
                                        for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_R+WIDTH_D_R); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = iptsL[mi]+track_Width[mi];
                                        rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                    }
                                        if(lost_line_r >= DETECT_LOST_LINE_ARC /*&& r_monotony==-1*/)
                                            {
                                                System_DataStruct.CIRCLE_Flag = CIRCLE_PREV_ARC;
                                            }
                            }
                        /*******************************************************************************************************************************************/
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_ARC) //圆弧路段
                            {

                                unsigned char result_arc;
                                for( mi=ROW_Start-10;mi>ROW_Start-rpts_num;mi--){
                                    if(iptsr[mi]<=COL_End-1)continue;
                                    else {result_arc = mi; break;}
                                }

                                //偏移补线
                                for(mi=ROW_Start;mi>result_arc ;mi--) //计算路宽 等于result_arc的话就把边缘的那个点也计算了
                                {
                                    track_Width[mi]=(unsigned char)(mi*WIDTH_P_ARC_R+WIDTH_D_ARC_R); //计算  路宽 mi*Width_P+24.6
#if PREV_ARC_MONOTONY==1
                                    rpts[mi] = (iptsr[mi]-track_Width[mi])<=0?0:(iptsr[mi]-track_Width[mi]);
#else
                                    if( (iptsr[mi]-track_Width[mi]) <= COL/2)
                                    {
                                        rpts[mi]=COL/2;
                                    }
                                    else
                                    {
                                        rpts[mi] = iptsr[mi]-track_Width[mi];
                                    }
#endif


                                }

                                //后续自增
                                for(mi=result_arc;mi>ROW_Start-rpts_num;mi--)
                                {
                                    if(rpts[mi+1]>=COL_End){rpts_num = ROW_Start-(mi+1);break;}
                                    rpts[mi] = rpts[mi+1]+PREV_ARC_INCREASE;
                                    rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                }

                                if(lost_line_r == -1)
                                {
                                    System_DataStruct.CIRCLE_Flag = CIRCLE_WILL_IN;

                                }

                            }
                        /*******************************************************************************************************************************************/
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_WILL_IN) //即将进入环岛
                            {
#if 0
                                unsigned char result_x,result_y;
                                for(mi=COL_End;mi>=Image_DataStruct.lst_cols;mi--)
                                {
                                    if(white_col[mi]==0)continue;
                                    else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                }
                                slope = (float)(result_y-ROW_Start)/(result_x-COL/2/*rpts[ROW_Start]*/);
                                intercept = ROW_Start-slope*COL/2/*rpts[ROW_Start]*/;
                                Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                rpts_num = ROW_Start-result_y;
#elif 1
                                rpts[ROW_Start]=COL/2;
                                for(mi=ROW_Start-1;mi>=1;mi--)
                                {
                                    if(rpts[mi+1]>=COL_End){rpts_num = ROW_Start-(mi+1);break;}
                                    rpts[mi] = rpts[mi+1]+2;
                                    rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                }
#endif

                                if(Image_DataStruct.begin_x>=COL/2/*Image_DataStruct.lst_rows <= 60*/ && l_monotony==1){System_DataStruct.CIRCLE_Flag = CIRCLE_HAVE_IN;}
                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_HAVE_IN)
                            {
                                if(Image_DataStruct.begin_x<=COL/2+10) //可能是由于光线忽明忽暗导致,这种情况是由于曝光没有设置好,但为了增加容错率,在这里十分有必要进行二次加工
                                {
//                                    gpio_toggle_level(BUZZER_PIN);
                                    rpts[ROW_Start]=COL/2;
                                    for(mi=ROW_Start-1;mi>=1;mi--)
                                    {
                                        if(rpts[mi+1]>=COL_End){rpts_num = ROW_Start-(mi+1);break;}
                                        rpts[mi] = rpts[mi+1]+3;
                                        rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                    }
                                }
                                else
                                {
                                    for ( mi = ROW_Start-5; mi >=ROW_Start-rpts_num; mi--) //末尾5行不计算
                                    {

                                        if(rpts[mi]<rpts[mi+1] && rpts[mi]<rpts[mi+2] ) //左边线减小趋势 这在右环岛不被允许
                                        {
                                            rpts[mi] = rpts[mi+1]+1;
                                            rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                        }
                                    }
                                }


                                for(mi = ROW_Start-10;mi>=ROW_Start-rpts_num;mi--)
                                {
                                    if(iptsL[mi]<=COL_Start)continue; //搜线错误,嘎嘎  //也可以最长白行

                                        if(iptsL[mi]<iptsL[mi+2] && iptsL[mi+1]<iptsL[mi+4]
                                        && iptsL[mi+4]>iptsL[mi+6] && iptsL[mi+5] > iptsL[mi+7])
                                        {

                                        System_DataStruct.CIRCLE_Flag = CIRCLE_OUT_D;break;}
                                }
                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_OUT_D)
                            {

                                for ( mi = ROW_Start-5; mi >=ROW_Start-rpts_num; mi--) //末尾5行不计算
                                {

                                    if(rpts[mi]<rpts[mi+1] && rpts[mi]<rpts[mi+2] ) //左边线减小趋势
                                    {
                                        rpts[mi] = (rpts[mi+1]+OUT_D_INCREASE)>=COL_End?COL_End:(rpts[mi+1]+OUT_D_INCREASE);
                                    }
                                }

                                if(/*lost_line_r==-1 && */lost_line_l==-1)
                                {
                                    unsigned char result_x,result_y;
                                    for(mi=COL_End;mi>=Image_DataStruct.begin_x;mi--)
                                    {
                                        if(white_col[mi]==0)continue;
                                        else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                    }
                                    slope = (float)(result_y-ROW_Start)/(result_x-COL/2);//rpts[ROW_Start
                                    intercept = ROW_Start-slope*COL/2;
                                    Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                    rpts_num = ROW_Start-result_y;
                                    System_DataStruct.CIRCLE_Flag=CIRCLE_TURN_D;
                                }

                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_TURN_D)
                            {
#if 0
                                    unsigned char result_x,result_y;
                                    for(mi=COL_End;mi>=Image_DataStruct.begin_x;mi--)
                                    {
                                        if(white_col[mi]==0)continue;
                                        else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                    }
                                    slope = (float)(result_y-ROW_Start)/(result_x-COL/2); //rpts[ROW_Start]
                                    intercept = ROW_Start-slope*COL/2;
                                    Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                    rpts_num = ROW_Start-result_y;
#elif 1
                                    rpts[ROW_Start]=COL/2;
                                    for(mi=ROW_Start-1;mi>=1;mi--) //无符号整型 mi始终为正
                                    {
                                        if(rpts[mi+1]>=COL_End){rpts_num = ROW_Start-(mi+1);break;}
                                        rpts[mi] = (rpts[mi+1]+2)>=COL_End?COL_End:(rpts[mi+1]+2);
                                    }
#endif
                                    if(lost_line_l>=DETECT_LOST_LINE_ARC  ){System_DataStruct.CIRCLE_Flag = CIRCLE_OUT_U;}
                            }

                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_OUT_U)
                            {
                                    /* 异路补线 */
                                    for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*WIDTH_P_OUT_R+WIDTH_D_OUT_R); //计算  路宽 mi*Width_P+24.6
                                        if(iptsL[mi]+track_Width[mi]<=COL/2)
                                        {
                                            rpts[mi]=COL/2;
                                        }
                                        else if((iptsL[mi]+track_Width[mi])>=COL_End)
                                        {
                                            rpts[mi]=COL_End;
                                        }
                                        else
                                        {
                                            rpts[mi] = iptsL[mi]+track_Width[mi];
                                        }

                                    }

                                if(lost_line_r>=15 /*没有丢线 */ && monotony_r(ROW_Start-5,ROW_Start-20)==1 /* 末尾10起步 全部内倾 */ )
                                {
                                    System_DataStruct.CIRCLE_Flag = ELEMENMT_NONE;
                                    System_DataStruct.Element_Flag = ELEMENMT_NONE;
                        //
                                    // printf("over\n");
                                }

                            }

    break;
/**********************************************************************左环岛************************************************************************************************/
/**********************************************************************左环岛************************************************************************************************/
    case LEFT_CIRCLE_IN:

                            lost_line_r = mislaid_r(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC); //119 100
                            lost_line_l = mislaid_l(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC);
                            r_monotony  = monotony_r(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC);
                            l_monotony  = monotony_L(ROW_Start,ROW_Start-DETECT_LOST_LINE_ARC); // test excess
                            if(lost_line_l !=-1 && System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_D_M) //没有全丢,使用合成的中线
                            {

                                    for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_L+WIDTH_D_L); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = (iptsr[mi]-track_Width[mi])<=0?0:(iptsr[mi]-track_Width[mi]);
                                    }

                            }
                        /*******************************************************************************************************************************************/
                            else if( (lost_line_l ==-1) && (System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_D_M ) ) //底部20行全丢 或者得线数少于3 下拐点和圆弧路段
                            {

                                System_DataStruct.CIRCLE_Flag=CIRCLE_PREV_M;
                                for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_L+WIDTH_D_L); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = (iptsr[mi]-track_Width[mi])<=0?0:(iptsr[mi]-track_Width[mi]);
                                    }

                            }
                            else if(System_DataStruct.CIRCLE_Flag==CIRCLE_PREV_M) //入圆弧前 //test excess
                            {
                        //            ips200_show_int(20, ROW,22, 3);
                                    for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*Width_P_L+WIDTH_D_L); //计算  路宽 mi*Width_P+24.6
                                        rpts[mi] = (iptsr[mi]-track_Width[mi])<=0?0:(iptsr[mi]-track_Width[mi]);
                                    }

                                    if(lost_line_l >= DETECT_LOST_LINE_ARC /*&& l_monotony==-1*/){System_DataStruct.CIRCLE_Flag = CIRCLE_PREV_ARC;}
                            }
                        /************************************************** 圆 弧 路 段 *****************************************************************************************/
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_PREV_ARC) //圆弧路段
                            {
                                //寻找圆弧截至点
                                unsigned char result_arc;
                                for( mi=ROW_Start-10;mi>ROW_Start-rpts_num;mi--){
                                    if(iptsL[mi]>=COL_Start+1)continue; //过滤非边界点
                                    else {result_arc = mi; break;}
                                }

                                for(mi=ROW_Start;mi>result_arc ;mi--) //计算路宽 等于result_arc的话就把边缘的那个点也计算了
                                {
                                    track_Width[mi]=(unsigned char)(mi*WIDTH_P_ARC_L+WIDTH_D_ARC_L); //计算  路宽 mi*Width_P+24.6
                                    if( (iptsL[mi]+track_Width[mi]) >=COL/2)
                                    {
                                        rpts[mi]=COL/2;
                                    }
                                    else
                                    {
                                        rpts[mi] = iptsL[mi]+track_Width[mi]; //圆弧路段偏移补线
                                    }
//                                    if(iptsL[mi]==COL_Start)continue;
//
//                                    rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                }

                                for(mi=result_arc;mi>ROW_Start-rpts_num;mi--)
                                {
                                    if(rpts[mi+1]<=COL_Start){rpts_num = ROW_Start-(mi+1);break;} //补到边界就停止
                                    rpts[mi] = (rpts[mi+1]-PREV_ARC_INCREASE)<=0?0:(rpts[mi+1]-PREV_ARC_INCREASE);
                                }

                                if(lost_line_l == -1)
                                {
                                    System_DataStruct.CIRCLE_Flag = CIRCLE_WILL_IN;

                                }

                            }
                        /*******************************************************************************************************************************************/
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_WILL_IN) //即将进入环岛
                            {
#if 0
                                unsigned char result_x,result_y;
                                for(mi=COL_Start;mi<=Image_DataStruct.begin_x;mi++)
                                {
                                    if(white_col[mi]==0)continue;
                                    else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                }
                                slope = (float)(result_y-ROW_Start)/(result_x-COL/2/*rpts[ROW_Start]*/);
                                intercept = ROW_Start-slope*COL/2/*rpts[ROW_Start]*/;
                                Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                rpts_num = ROW_Start-result_y;
#elif 1
                                rpts[ROW_Start]=COL/2;
                                for(mi=ROW_Start-1;mi>=ROW_Start-rpts_num;mi--)
                                {
                                    if(rpts[mi+1]<=COL_Start){rpts_num = ROW_Start-(mi+1);break;}
                                    rpts[mi] = (rpts[mi+1]-2)<=0?0:(rpts[mi+1]-2);
//                                    rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                }
#endif

                                if(Image_DataStruct.begin_x <= COL/2 && r_monotony==1){System_DataStruct.CIRCLE_Flag = CIRCLE_HAVE_IN;}
                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_HAVE_IN)
                            {
                                if(Image_DataStruct.begin_x >= COL/2-10)
                                {
                                    rpts[ROW_Start]=COL/2;
                                    for(mi=ROW_Start-1;mi>=1;mi--)
                                    {
                                        if(rpts[mi+1]<=COL_Start){rpts_num = ROW_Start-(mi+1);break;}
                                        rpts[mi] = (rpts[mi+1]-3)<=0?0:(rpts[mi+1]-3);
                                    }
                                }
                                else
                                {
                                    for ( mi = ROW_Start-5; mi >=ROW_Start-rpts_num; mi--) //末尾5行不计算
                                    {
                                        if(rpts[mi]>rpts[mi+1] && rpts[mi]>rpts[mi+2] ) //边线增加趋势
                                        {
                                            rpts[mi] = (rpts[mi+1]-2)<=0?0:(rpts[mi+1]-2); //强制减小
                                        }
                                    }
                                }


                                for(mi = ROW_Start-10;mi>=ROW_Start-rpts_num;mi--)
                                {
                                    if(iptsr[mi]>=COL_End)continue; //搜线错误,嘎嘎，弯道路段右边线不可能出现跑到右边界的情况,如果出现，就是干扰

                                        if(iptsr[mi]>iptsr[mi+2] && iptsr[mi+1]>iptsr[mi+4]
                                        && iptsr[mi+4]<iptsr[mi+6] && iptsr[mi+5] < iptsr[mi+7])
                                        {

                                        System_DataStruct.CIRCLE_Flag = CIRCLE_OUT_D;break;}
                                }
                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_OUT_D)
                            {

                                for ( mi = ROW_Start-5; mi >=ROW_Start-rpts_num; mi--) //末尾5行不计算
                                {

                                    if(rpts[mi]>rpts[mi+1] && rpts[mi]>rpts[mi+2] ) //边线增加趋势
                                    {
                                        rpts[mi] = (rpts[mi+1]-OUT_D_INCREASE)<=0?0:(rpts[mi+1]-OUT_D_INCREASE);
//                                        rpts[mi] = func_limit_ab(rpts[mi],COL_Start,COL_End);
                                    }
                                }

                                if(/*lost_line_r==-1 && */lost_line_r==-1)
                                {
                                    unsigned char result_x,result_y;
                                    for(mi=COL_Start;mi<=Image_DataStruct.begin_x;mi++)
                                    {
                                        if(white_col[mi]==0)continue;
                                        else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                    }
                                    slope = (float)(result_y-ROW_Start)/(result_x-COL/2); //rpts[ROW_Start]
                                    intercept = ROW_Start-slope*COL/2; //rpts[ROW_Start]
                                    Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                    rpts_num = ROW_Start-result_y;
                                    System_DataStruct.CIRCLE_Flag=CIRCLE_TURN_D;
                                }

                            }
                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_TURN_D)
                            {
#if 0
                                    unsigned char result_x,result_y;
                                    for(mi=COL_Start;mi<=Image_DataStruct.begin_x;mi++)
                                    {
                                        if(white_col[mi]==0)continue;
                                        else {result_x = mi;result_y=ROW_Start-white_col[mi]; break;}
                                    }
                                    slope = (float)(result_y-ROW_Start)/(result_x-COL/2); //rpts[ROW_Start]
                                    intercept = ROW_Start-slope*COL/2;//rpts[ROW_Start]
                                    Patch_Line_Points(ROW_Start,result_y,slope,intercept,rpts);
                                    rpts_num = ROW_Start-result_y;
#elif 1
                                    rpts[ROW_Start]=COL/2;
                                    for(mi=ROW_Start-1;mi>=1;mi--)
                                    {
                                        if(rpts[mi+1]<=COL_Start){rpts_num = ROW_Start-(mi+1);break;}
                                        rpts[mi] = (rpts[mi+1]-2)<=0?0:(rpts[mi+1]-2);
                                    }
#endif

                                    if(/*(ROW_Start-rpts_num)<=(70) && */lost_line_r>=DETECT_LOST_LINE_ARC){System_DataStruct.CIRCLE_Flag = CIRCLE_OUT_U;}//如果出弯就沿着异路边线走 可能会导致 偏移得到的中线一半在左 一半在右 导致误差较小 拐弯不及时 所以需要要求异路边线末尾的行数小于一定距离
                            }

                            else if(System_DataStruct.CIRCLE_Flag == CIRCLE_OUT_U)
                            {
                                    /* 异路补线 */
                                    for(mi=ROW_Start;mi>=ROW_Start-rpts_num ;mi--) //计算路宽
                                    {
                                        track_Width[mi]=(unsigned char)(mi*WIDTH_P_OUT_L+WIDTH_D_OUT_L); //计算  路宽 mi*Width_P+24.6
                                        if((iptsr[mi]-track_Width[mi])>=COL/2)
                                        {
                                            rpts[mi]=COL/2;
                                        }
                                        else if((iptsr[mi]-track_Width[mi])<=0)
                                        {
                                            rpts[mi]=0;
                                        }
                                        else
                                        {
                                            rpts[mi] = (iptsr[mi]-track_Width[mi]);
                                        }
//                                        rpts[mi] = (iptsr[mi]-track_Width[mi])<=0?0: (iptsr[mi]-track_Width[mi]) ;
//                                        rpts[mi] = (iptsr[mi]-track_Width[mi])>=COL/2?COL/2:(iptsr[mi]-track_Width[mi]);

                                    }
                                if(lost_line_l>=15 /*没有丢线 */ && monotony_L(ROW_Start-5,ROW_Start-20)==1 /* 末尾10起步 全部内倾 */ )
                                {
                                    System_DataStruct.CIRCLE_Flag = ELEMENMT_NONE;
                                    System_DataStruct.Element_Flag = ELEMENMT_NONE;
                        //                gpio_set_level(BUZZER_PIN,0);
                                    // printf("over\n");
                                }

                            }
    break;
    default:
    break;

}

}
#undef DETECT_LOST_LINE_ARC 


/// @brief 某点曲率计算 左线负数 右线正数
/// @param mid 
/// @param step 
/// @param length 
/// @return 当点的曲率值
int detect_icurvity(unsigned char mid,unsigned char step,unsigned char length,bool way)
{
switch(way)
{
    case 0:
        find_line_l(mid+step,length);
    return (process_icurvity(ipts0,0,step)); //没有做限幅保护,如果2*INCURVITY大于边线长度
break;
    case 1:
        find_line_r(mid+step,length);
    // unsigned char end=ipts1_num-1;
    return (process_icurvity(ipts1,0,step)); //没有做限幅保护,如果2*INCURVITY大于边线长度
break;
}

}


unsigned char Garage_find (unsigned char col_begin,unsigned char col_end) //入库判断
{
    unsigned char flag_count = 0;
    unsigned char row, col;
if(col_begin<=COL_Start)col_begin=COL_Start;
if(col_end>=COL_End)col_end=COL_End;

    if(System_DataStruct.Element_Flag == ELEMENMT_NONE || System_DataStruct.Element_Flag == GARAGE_FIND)
    {
        for(row = 80; row >= 79; row--)     //扫描两行获取跳变点数量
        {
           for(col = col_begin; col < col_end - 1; col++)
           {
               if(Diff_Ratio(Gray_Image[row][col],Gray_Image[row][col+3])>=Image_DataStruct.Diff_Ratio)
               {
                   flag_count++;
               }
           }
        }
    }

    if(flag_count >= 7*2) //7*2      //判断对应跳变点数量
    {
        System_DataStruct.Element_Flag = GARAGE_FIND;
//        gpio_toggle_level(BUZZER_PIN);

        return 1;
    }

    return 0;
}